import io
from picamera import PiCamera
import time
import numpy as np
import cv2
import imgprocess
import pwm
import search
import math

norm_speed=45
camera = PiCamera()
camera.resolution = (640, 480)
car=pwm.CarMove()

stream = io.BytesIO()
cnt=0
count=0
angle=0
for frame in camera.capture_continuous(stream, format='jpeg', use_video_port=True):
    cnt=(cnt+1)%10
    count+=1
    data = np.fromstring(stream.getvalue(), dtype=np.uint8)
    image = cv2.imdecode(data,cv2.IMREAD_COLOR)
    dst = search.initial_process(image)
    height, width = dst.shape
    base=int(height / 3*2)
    angle= search.decision_k(dst,base,angle,count)
    # if car.get_dis()<0.2:
    #     pwm.control(-norm_speed,angle,car)
    #     time.sleep(1)
    # print("转向角度",angle)
    cv2.imwrite("img/img"+str(count)+"转向角度："+str(angle)+".jpg",image)
    cv2.imwrite("img/dst"+str(count)+"转向角度："+str(angle)+".jpg",dst)
    cv2.imwrite("static/videos/frame"+str(cnt)+".jpg",image)
    cv2.imwrite("static/pandas/frame_dst"+str(cnt)+".jpg",dst)
    pwm.control(norm_speed,angle,car)
    # cv2.imshow("img",dst)
    # cv2.waitKey(10)
    stream.truncate()
    stream.seek(0)
